#include "MediaFoundationCaptureCam.h"
#include <iostream>
#include <omp.h>

#include <fstream>
#include <QString>

void stopCaptureEvent(int deviceID, void *userData)
{
    videoInput* capture = &videoInput::getInstance();

    capture->closeDevice(deviceID);
}

MediaFoundationCaptureCam::MediaFoundationCaptureCam(int captureDeviceIdx)
    : m_countLeftFrames(0),
      m_rawFrameData(0),
      m_prevFrameData(0)
{
    m_captureDeviceIdx = captureDeviceIdx;
    m_captureDeviceFormatIdx = 0;
    m_frameData = 0;
}

MediaFoundationCaptureCam::~MediaFoundationCaptureCam()
{
    if(m_capture)
    {
        m_capture->closeDevice(m_captureDeviceIdx);
    }

    if(m_frameData)
    {
        delete m_frameData;
        m_frameData = 0;
    }

    if(m_prevFrameData)
    {
        delete m_frameData;
        m_frameData = 0;
    }
}

bool MediaFoundationCaptureCam::init()
{
    m_capture = &videoInput::getInstance();
    int nDevices = m_capture->listDevices(true);

    if(nDevices < 1)
    {
        return false;
    }

    if(!m_capture->setupDevice(m_captureDeviceIdx, 640, 480, 30))
    {
        std::cerr << "Could not initialize device\n";
        return false;
    }

    m_capture->setEmergencyStopEvent(m_captureDeviceIdx, NULL, stopCaptureEvent);
    createResources(640, 480);
    if(!m_capture->isFrameNew(m_captureDeviceIdx))
    {
        std::cout << "No new frame\n";
    }
    else
    {
        std::cout << "New frame\n";
    }

    return true;
}

bool MediaFoundationCaptureCam::changeCaptureDevice(int captureDeviceIdx, int captureDeviceFormatIdx)
{
    if(m_capture)
    {
        m_capture->closeDevice(m_captureDeviceIdx);
    }

    m_capture = &videoInput::getInstance();
    int nDevices = m_capture->listDevices(true);

    // just for safety
    if(nDevices < 1)
    {
        return false;
    }

    if(!m_capture->setupDevice(captureDeviceIdx, captureDeviceFormatIdx))
    {
        std::cerr << "Could not initialize device\n";
        return false;
    }

    m_captureDeviceIdx = captureDeviceIdx;
    m_captureDeviceFormatIdx = captureDeviceFormatIdx;

    m_capture->setEmergencyStopEvent(m_captureDeviceIdx, NULL, stopCaptureEvent);
    createResources(m_capture->getWidth(m_captureDeviceIdx), m_capture->getHeight(m_captureDeviceIdx));
    emit frameSizeChanged(m_capture->getWidth(m_captureDeviceIdx), m_capture->getHeight(m_captureDeviceIdx));

    return true;
}

unsigned char* MediaFoundationCaptureCam::getFrame()
{
    if(!m_capture->isDeviceSetup(m_captureDeviceIdx))
    {
        std::cerr << "Device setup failed.\n" << std::endl;
        return NULL;
    }

    if(m_capture->isFrameNew(m_captureDeviceIdx))
    {
        m_capture->getPixels(m_captureDeviceIdx, m_rawFrameData, false, false);
        
        #pragma omp parallel for
        for(int offset = 0; offset < frameWidth() * frameHeight(); ++offset)
        {
            m_frameData[offset * 4 + 0] = m_rawFrameData[offset * 3 + 0];
            m_frameData[offset * 4 + 1] = m_rawFrameData[offset * 3 + 1];
            m_frameData[offset * 4 + 2] = m_rawFrameData[offset * 3 + 2];
            m_frameData[offset * 4 + 3] = 255;
        }

        if(isFrameIdentical())
        {
            // nothing new was grabbed
            return NULL;
        }
        return m_prevFrameData;
    }
    return NULL;
}

bool MediaFoundationCaptureCam::isFrameIdentical()
{
    // check frame identity
    bool state = true;

    #pragma omp parallel for
    for(int offset = 0; offset < frameWidth() * frameHeight(); ++offset)
    {
        if(m_frameData[offset * 4 + 0] != m_prevFrameData[offset * 4 + 0])
        {
            state = false;
        }

        if(m_frameData[offset * 4 + 1] != m_prevFrameData[offset * 4 + 1])
        {
            state = false;
        }

        if(m_frameData[offset * 4 + 2] != m_prevFrameData[offset * 4 + 2])
        {
            state = false;
        }
    }

    // swap
    if(state == false)
    {
        swapFrameBuffers(m_frameData, m_prevFrameData);
    }
    return state;
}

int MediaFoundationCaptureCam::frameWidth()
{
    return m_capture->getWidth(m_captureDeviceIdx);
}

int MediaFoundationCaptureCam::frameHeight()
{
    return m_capture->getHeight(m_captureDeviceIdx);
}

int MediaFoundationCaptureCam::getCaptureFPS()
{
    MediaType type = m_capture->getFormat(m_captureDeviceIdx, m_captureDeviceFormatIdx);
    return static_cast<int>(type.MF_MT_FRAME_RATE);
}

int MediaFoundationCaptureCam::listCaptureDevices()
{
    return m_capture->listDevices();
}

void MediaFoundationCaptureCam::createResources(int width, int height)
{
    if(m_frameData)
    {
        delete m_frameData;
        m_frameData = 0;
    }

    if(m_prevFrameData)
    {
        delete m_prevFrameData;
        m_prevFrameData = 0;
    }

    if(m_rawFrameData)
    {
        delete m_rawFrameData;
        m_rawFrameData = 0;
    }

    m_rawFrameData = new unsigned char[width * height * 3];
    m_frameData = new unsigned char[width * height * 4];
    m_prevFrameData = new unsigned char[width * height * 4];
    memset(m_prevFrameData, 0, sizeof(unsigned char) * width * height * 4);
}